#include <iostream>

#include <boost/format.hpp>

#include <tf/transform_datatypes.h>

#include "youbot_relay/move_youbot_base_server.h"
#include <tf/transform_datatypes.h>

using namespace std;

std::ostream& operator<<(ostream& stream, const geometry_msgs::Pose& pose)
{
  double yaw = tf::getYaw(pose.orientation);
  return stream << boost::format("[%.3f; %.3f; %.3f]") % pose.position.x % pose.position.y % yaw;
}

namespace move_youbot_base
{

MoveYoubotBaseServer::MoveYoubotBaseServer(ros::NodeHandle n, tf::TransformListener& tf) :
  node_(n), action_server_(NULL), transform_listener_(tf), controller_frequency_(10),
  global_frame_id_("odom"),
  youbot_base_frame_id_("base_footprint"),
  max_translational_velocity_(0.1),
  min_translational_velocity_(0.01),
  max_rotational_velocity_(0.1),
  min_rotational_velocity_(0.01)
{
  ROS_INFO("Creating action server.");
  action_server_ = new MoveYoubotBaseActionServer(ros::NodeHandle(), "move_youbot_base",
                                                  boost::bind(&MoveYoubotBaseServer::execute, this, _1), false);
  ROS_INFO("Creating velocity publisher.");
  velocity_publisher_ = node_.advertise<geometry_msgs::Twist> ("cmd_vel", 1);
  action_server_->start();
}

MoveYoubotBaseServer::~MoveYoubotBaseServer()
{
  ROS_INFO("Deleting action server.");
  if (action_server_ != NULL)
    delete action_server_;
}

void MoveYoubotBaseServer::publishVelocity(double x, double y, double z)
{
  geometry_msgs::Twist velocity_command;
  velocity_command.linear.x = x;
  velocity_command.linear.y = y;
  velocity_command.angular.z = z;
  velocity_publisher_.publish(velocity_command);
}

void MoveYoubotBaseServer::publishZeroVelocity()
{
  publishVelocity(0, 0, 0);
}

double MoveYoubotBaseServer::calculateDistance(const geometry_msgs::PoseStamped& p1,
                                               const geometry_msgs::PoseStamped& p2)
{
  double dx = p1.pose.position.x - p2.pose.position.x;
  double dy = p1.pose.position.y - p2.pose.position.y;
  return sqrt(dx * dx + dy * dy);
}

double MoveYoubotBaseServer::percentageOfRange(double min, double max, int percentage)
{
  ROS_ASSERT(percentage >= 0 && percentage <= 100);
  ROS_ASSERT(min < max);
  return min + (max - min) * percentage / 100;
}

bool MoveYoubotBaseServer::isQuaternionValid(const geometry_msgs::Quaternion& q)
{
  // ensure that the quaternion does not have NaN's or infinities
  if (!std::isfinite(q.x) || !std::isfinite(q.y) || !std::isfinite(q.z) || !std::isfinite(q.w))
  {
    ROS_ERROR("Quaternion has NaN's or infinities. Discarding as a navigation goal.");
    return false;
  }

  tf::Quaternion tf_q(q.x, q.y, q.z, q.w);

  // ensure that the length of the quaternion is not close to zero
  if (tf_q.length2() < 1e-6)
  {
    ROS_ERROR("Quaternion has length close to zero. Discarding as navigation goal.");
    return false;
  }

  // normalize the quaternion and check that it transforms the vertical vector correctly
  tf_q.normalize();
  tf::Vector3 up(0, 0, 1);
  double dot = up.dot(up.rotate(tf_q.getAxis(), tf_q.getAngle()));
  if (fabs(dot - 1) > 1e-3)
  {
    ROS_ERROR("The z-axis of the quaternion is not close to vertical. Discarding as navigation goal.");
    return false;
  }

  return true;
}

bool MoveYoubotBaseServer::setNewGoal(const youbot_relay::MoveYoubotBaseGoalConstPtr& new_goal)
{
  ROS_INFO_STREAM("Requested position relative to robot: " << new_goal->target_pose.pose);

  if (!isQuaternionValid(new_goal->target_pose.pose.orientation))
  {
    ROS_WARN("Aborted. Goal has an invalid quaternion.");
    action_server_->setAborted(youbot_relay::MoveYoubotBaseResult(), "Aborting. Goal has an invalid quaternion.");
    return false;
  }

  // calculate the goal pose in global frame
  global_goal_ = transformPose(new_goal->target_pose, youbot_base_frame_id_, global_frame_id_);

  ROS_INFO_STREAM("Requested position in global frame: " << global_goal_.pose);

  int velocity = new_goal->velocity > 100 ? 100 : new_goal->velocity;
  translational_velocity_ = percentageOfRange(min_translational_velocity_, max_translational_velocity_, velocity);
  rotational_velocity_ = percentageOfRange(min_rotational_velocity_, max_rotational_velocity_, velocity);

  ROS_INFO("Velocity: %d%% (%.3f m/s translation, %.3f rad/s rotation).", velocity, translational_velocity_, rotational_velocity_);

  return true;
}

geometry_msgs::PoseStamped MoveYoubotBaseServer::transformPose(const geometry_msgs::PoseStamped& pose_msg,
                                                               const std::string& source_frame_id,
                                                               const std::string& target_frame_id)
{
  tf::Stamped<tf::Pose> pose, target_pose;
  poseStampedMsgToTF(pose_msg, pose);
  pose.stamp_ = ros::Time();
  pose.frame_id_ = source_frame_id; // assume that the pose is given in the source_frame
  try
  {
    transform_listener_.transformPose(target_frame_id, pose, target_pose);
  }
  catch (tf::TransformException& ex)
  {
    ROS_WARN(
        "Pose transform failed (%s -> %s). Reason: %s.", source_frame_id.c_str(), target_frame_id.c_str(), ex.what());
    return pose_msg;
  }
  geometry_msgs::PoseStamped target_pose_msg;
  tf::poseStampedTFToMsg(target_pose, target_pose_msg);
  return target_pose_msg;
}

void MoveYoubotBaseServer::execute(const youbot_relay::MoveYoubotBaseGoalConstPtr& move_youbot_base_goal)
{
  ROS_INFO("Move base action callback.");

  if (!setNewGoal(move_youbot_base_goal))
    return;

  ros::Rate rate(controller_frequency_);
  while (node_.ok())
  {
    // process pending preemption requests
    if (action_server_->isPreemptRequested())
    {
      ROS_INFO("Action preemtion requested.");
      if (action_server_->isNewGoalAvailable() && setNewGoal(action_server_->acceptNewGoal()))
      {
        // a new valid goal was given and accepted
        // notify the ActionServer that we preempted and proceed to execute the action
        action_server_->setPreempted();
      }
      else
      {
        // we have been preempted explicitly, without a new goal
        // therefore we need to shut things down
        publishZeroVelocity();
        // notify the ActionServer that we have successfully preempted
        action_server_->setPreempted();
        // no goal - no actions, just exit the callback
        return;
      }
    }

    // issue a command to move youbot base towards the current goal
    // ros::Duration time_left;
    if (moveTowardsGoal(/*time_left*/))
    {
      // finish execution if the goal was reached
      action_server_->setSucceeded(youbot_relay::MoveYoubotBaseResult(), "Goal reached.");
      publishZeroVelocity();
      return;
    }
    else
    {
      // publish the feedback
      // youbot_relay::MoveYoubotBaseActionFeedback feedback;
      // feedback.estimated_time_left = time_left;
      // action_server_->publishFeedback(static_cast<FeedbackConstPtr>(feedback));
    }

    rate.sleep();
  }

  // if the node is killed then we abort and return
  action_server_->setAborted(youbot_relay::MoveYoubotBaseResult(), "Aborting. The node has been killed.");
  return;
}

bool MoveYoubotBaseServer::moveTowardsGoal(/*ros::Duration& time_left*/)
{
  geometry_msgs::PoseStamped goal = transformPose(global_goal_, global_frame_id_, youbot_base_frame_id_);
  ROS_DEBUG_STREAM("Moving towards the goal at " << goal.pose << " in robot coordinate frame.");

  double remains[3] = { goal.pose.position.x, goal.pose.position.y, tf::getYaw(goal.pose.orientation) };
  double tolerance[3] = { translational_tolerance_, translational_tolerance_, rotational_tolerance_ };
  double max_velocity[3] = { translational_velocity_, translational_velocity_, rotational_velocity_ };
  double velocity[3];
  bool reached[3];
  bool goal_reached = true;
  double remaining_time = 0;

  for (int i = 0; i < 3; ++i)
  {
    double time = std::abs(remains[i]) / max_velocity[i];
    remaining_time = time > remaining_time ? time : remaining_time;
    reached[i] = std::abs(remains[i]) < tolerance[i];
    goal_reached &= reached[i];
  }

  if (goal_reached)
  {
    ROS_INFO("The goal was reached.");
    return true;
  }

  for (int i = 0; i < 3; ++i)
  {
    velocity[i] = reached[i] ? 0 : remains[i] / remaining_time;
  }

  ROS_DEBUG("Issuing a command to move towards the goal: [%.3f; %.3f; %.3f]", velocity[0], velocity[1], velocity[2]);
  publishVelocity(velocity[0], velocity[1], velocity[2]);
  //time_left = ros::Duration::fromSec(remaining_time);
  return false;
}

}
;

int main(int argc, char **argv)
{
  ros::init(argc, argv, "move_youbot_base_server");
  ros::NodeHandle node;
  tf::TransformListener tf(ros::Duration(10)); // this value was taken from move_base. TODO: find what does it mean.

  move_youbot_base::MoveYoubotBaseServer move_youbot_base_server(node, tf);
  ros::spin();
  return 0;
}
